/*
 * mox_flasher.c
 *
 *  Created on: Jun 14, 2010
 *      Author: Jason Monk
 */

#include "mox.h"
#include "mox_log.h"
#include "mox_module.h"
#include "mox_slave.h"
#include <print.h>
#include <stdlib.h>
#include "mox_flasher.h"

void flasher_master(mox_master_state *state)
{
	struct mox_flash_entry flasher = {
			MOD_NAME, //module call name
			{ 0, 0, 0, 0 }, //empty function pointers
			{MOD_OFFSET_0 - MOD_START_0, MOD_OFFSET_1 - MOD_START_1, MOD_OFFSET_2 - MOD_START_2, MOD_OFFSET_3 - MOD_START_3}, //func start
			0, //size count
			{ MOD_STACK_0, MOD_STACK_1, MOD_STACK_2, MOD_STACK_3 }, //stack
			{ MOD_CP_0 - MOD_START_0, MOD_CP_1 - MOD_START_1, MOD_CP_2 - MOD_START_2, MOD_CP_3 - MOD_START_3 }, //cp
			{ MOD_DP_0 - MOD_START_0, MOD_DP_2 - MOD_START_1, MOD_DP_2 - MOD_START_2, MOD_DP_3 - MOD_START_3 } //dp
	};
	void *func_space;
	int index;
	int size[4];
	unsigned response;
	mox_msg msg;
	int i, j;

#ifdef MOD_FUNC_0
	if ((int)MOD_FUNC_0 - MOD_START_0 - flasher.offset[0]) {
		master_log(state, "Offset Incorrect: Module name not updated or flasher entry filled incorrectly");
	}
#endif

	for (i = 0; i < NUM_CORES; i++)
	{
		msg.type = FLASH_GET_FUNC;
		msg.size = 0;
		msg.content = NULL;
		mox_outct(state->chan_mox[i], MOX_MSG);
		mox_comm_msg_send(state->chan_mox[i], msg);
		msg.type = 0;
		mox_comm_msg_recv_ref(state->chan_mox[i], &msg);
		if (msg.type != FLASH_ACK)
		{
			master_log(state, "Serious Error: Please Fix");
			return;
		}
		size[i] = ((unsigned *)msg.content)[0];
		flasher.size += size[i];
	}

	func_space = malloc(flasher.size);
	if (!func_space)
	{
		master_log(state, "Serious Error: Please Fix");
		return;
	}

	index = 0;
	for (i = 0; i < NUM_CORES; i++)
	{
		msg.type = FLASH_ACK;
		msg.size = 0;
		msg.content = NULL;
		for (j = index; j < (index + (size[i] / 4)); j++)
		{
			((int *) func_space)[j] = mox_chan_in_wd(state->chan_mox[i]);
		}
		flasher.addr[i] = (void *) (4 * index);
		index += (size[i] / 4);
		mox_comm_msg_send(state->chan_mox[i], msg);
		msg.type = 0;
		mox_comm_msg_recv_ref(state->chan_mox[i], &msg);
		if (msg.type != FLASH_ACK)
		{
			master_log(state, "Serious Error: Please Fix");
			return;
		}
	}

	flash_func(state, &flasher, func_space, state->storage->mod[0]);

	return;
}

void flash_func(mox_master_state *state, struct mox_flash_entry *flasher,
		void *func_space, struct mox_module *storage)
{
	int i;
	int num_modules;
	char *db;
	struct mox_flash_entry *last_module;
	unsigned response;
	unsigned fn_start;
	unsigned db_start;
	unsigned tmp;
	unsigned db_cnt_start;
	unsigned clear_addr[] = {
			FN_ADDR,
			DB_ADDR
	};
	unsigned clear_n = sizeof(clear_addr)/sizeof(unsigned);

	if (!storage_free(storage)) {
		master_log(state, "Error: Storage device not free\n");
		return;
	}

	if (lock_storage(storage)) {
		master_log(state, "Error: Storage device could not lock\n");
		return;
	}

	if (CLEAR) {

		for (i = 0; i < NUM_CORES; i++) {
			flasher->addr[i] = (FN_ADDR + (void*)flasher->addr[i]);
		}
		if (clear_storage(storage, clear_addr, clear_n)) {
			master_log(state, "Error: Storage device could not read\n");
			return;
		}
		master_log(state, "Connected and Preparing to write to flash\n");


		fn_start = FN_ADDR;
		db_cnt_start = DB_ADDR;
		db_start = DB_ADDR + DB_OFFSET;


	} else {
		db = malloc(DB_OFFSET);
		if (!db) {
			master_log(state, "Error: Unable to allocate space for function DB, Aborting\n");
			return;
		}
		num_modules = 0;
		while (db[num_modules++] == 0);

		last_module = (struct mox_flash_entry *)read_storage(storage, DB_ADDR + DB_OFFSET +
				((num_modules-1)*sizeof(struct mox_flash_entry)),
				sizeof(struct mox_flash_entry));
		if (!last_module) {
			free(db);
			master_log(state, "Error: Could not get last function DB entry, Aborting\n");
			return;
		}

		fn_start = (int)last_module->addr[0] + last_module->size + 1;
		if (fn_start % 256) {
			fn_start += (256 - (fn_start % 256));
		}

		db_cnt_start = DB_ADDR + num_modules;
		db_start = DB_ADDR + DB_OFFSET + (num_modules * sizeof(struct mox_flash_entry));

		free(db);
		master_log(state, "Warning: This has not been fully implemented yet, May not be fully operational or tested\n");
	}

	tmp = write_storage(storage, fn_start, flasher->size, func_space);
	if (tmp) {
		master_log(state, "Error: Storage device could not write to memory");
		printintln(tmp);
		return;
	}

	master_log(state,
			"Completed writing function space, moving onto to DB\n");

	i = 0;
	if (write_storage(storage, db_cnt_start, 1, &i)) {
		master_log(state, "Error: Storage device could not write to memory");
		return;
	}

	if (write_storage(storage, db_start,
			sizeof(struct mox_flash_entry), flasher)) {
		master_log(state, "Error: Storage device could not write to memory");
		return;
	}

	master_log(state, "Completed Writing module to memory\n");

	if (unlock_storage(storage)) {
		master_log(state, "Error: Storage device could not unlock");
	}

	return;
}

void flasher_slave(struct mox_slave_state *state)
{
	int sizes[NUM_CORES] = {
			MOD_SIZE_0,
			MOD_SIZE_1,
			MOD_SIZE_2,
			MOD_SIZE_3
	};
	void *funcs[NUM_CORES] = {
			(void *) MOD_START_0,
			(void *) MOD_START_1,
			(void *) MOD_START_2,
			(void *) MOD_START_3
	};
	int i;
	unsigned response[4];
	mox_msg msg;

	msg.type = FLASH_ACK;
	msg.size = 1 * sizeof(unsigned);
	msg.content = response;
	response[0] = sizes[state->core_id];

	mox_comm_msg_send(state->mast, msg);

	for (i = 0; i < (sizes[state->core_id]) / 4; i++)
	{
		mox_chan_out_wd(state->mast, ((int *) funcs[state->core_id])[i]);
	}

	mox_comm_msg_recv_ref(state->mast, &msg);
	if (msg.type != FLASH_ACK)
	{
		msg.type = FLASH_NACK;
		printstr("Serious Error Core ");
		printintln(state->core_id);
	}
	mox_comm_msg_send(state->mast, msg);

	return;
}
